/** ###################################################################
**     Filename  : speed_control.c
**     Project   : ccd2
**     Processor : MCF52255CAF80
**     Version   : Driver 01.00
**     Compiler  : CodeWarrior MCF C Compiler
**     Arthor	 : Yumi Chan, Cindy Yu
**     Abstract  :
**         
**     Settings  :
**     Contents  :
**         No public methods
**
** ###################################################################*/


/* Include */
#include "speed_control.h"

MOTOR motor;

void speedControl()
{
	speedPID.error[2] = speedPID.error[1];
  	speedPID.error[1] = speedPID.error[0];
  	
  	speedPID.error[CURRENT] = distance - speed_set_value/SPEED_CHECK_FREQ;
  	speedPID.controller[_P] = (int)(speedPID.constant[Kp] * (speedPID.error[CURRENT] - speedPID.error[1]));
  	speedPID.controller[_I] = (speedPID.constant[Ki] * (speedPID.error[CURRENT]));
  	//speedPID.controller[_D] = (speedPID.constant[Kd] * (speedPID.error[CURRENT] - 2*speedPID.error[1]+speedPID.error[2]));
  	motor_duty += (speedPID.controller[_P] + speedPID.controller[_I]);
  	  	  	
  	if (motor_duty > 500)
  	  motor_PWM_SetDutyUS(500);	
  	else if (motor_duty < 0)
  	  motor_PWM_SetDutyUS(0);
  	else
  	  motor_PWM_SetDutyUS(motor_duty);
  	
	//printf("Plot:%d,%d\r\n",distance,motor_duty);
	//printf("%i\n",distance);
	
	return ;
}

